#!/usr/bin/env python3

#[ROS]动态参数设置-可视化调试-创建cfg文件
#https://blog.csdn.net/weixin_43728093/article/details/120004444
PACKAGE = 'contnav_global_planner'
#导入dynamic_reconfigure功能包提供参数生成器
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
#通过gen = ParameterGenerator()创建生成器。这里定义了四个不同类型的参数：int_t、double_t、str_t、bool_t。使用参数生成器的add(name, type, level, description, default, min, max) 方法生成参数，具体用法如下：
gen = ParameterGenerator()


#add(name, type, level, description, default, min, max) 方法生成参数，具体用法如下：
#name : 参数名，使用字符串描述 type: 定义参数类型，可以是int_t, double_t, str_t, bool_t
#level: 需要传入参数动态配置回调函数中的掩码，在回调函数中会修改所有参数的掩码，表示参数已经进行修改。
#description: 描述参数作用的字符串
#default: 设置参数的默认值。
#min: 可选，设置参数的最小值，对于字符串和布尔类型值不生效。
#max: 可选，设置参数的最大值，对于字符串和布尔类型值不生效。
#然后利用gen.enum方法生成一个枚举类型值，最后通过exit生成所有与C++和python相关的文件并推出程序。
# chmod a+x cfg/ContnavGlobalPlanner.cfg   chmod 777 ContnavGlobalPlanner.cfg
#注意： cfg/ContnavGlobalPlanner.cfg  这个文件放在 cmakelist的 find_package 下面调用

gen.add("lethal_cost", int_t, 0, "Lethal Cost", 253, 1, 255)
gen.add("neutral_cost", int_t, 0, "Neutral Cost", 50, 1, 255)
gen.add("cost_factor", double_t, 0, "Factor to multiply each cost from costmap by", 3.0, 0.01, 5.0)
gen.add("publish_potential", bool_t, 0, "Publish Potential Costmap", True)

'''
gen.add("base_global_planner", str_t, 0, "The name of the plugin for the global planner to use with move_base.", "navfn/NavfnROS")
gen.add("base_local_planner", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS")

#gen.add("recovery_behaviors", str_t, 0, "A list of recovery behavior plugins to use with move_base.", "[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]")

gen.add("planner_frequency", double_t, 0, "The rate in Hz at which to run the planning loop.", 0, 0, 100)
gen.add("controller_frequency", double_t, 0, "The rate in Hz at which to run the control loop and send velocity commands to the base.", 20, 0, 100)
gen.add("planner_patience", double_t, 0, "How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.", 5.0, 0, 100)
gen.add("controller_patience", double_t, 0, "How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.", 5.0, 0, 100)
gen.add("max_planning_retries", int_t, 0, "How many times we will recall the planner in an attempt to find a valid plan before space-clearing operations are performed", -1, -1, 1000)
gen.add("conservative_reset_dist", double_t, 0, "The distance away from the robot in meters at which obstacles will be cleared from the costmap when attempting to clear space in the map.", 3, 0, 50)

gen.add("recovery_behavior_enabled", bool_t, 0, "Whether or not to enable the move_base recovery behaviors to attempt to clear out space.", True)
# Doesnt exist
gen.add("clearing_rotation_allowed", bool_t, 0, "Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space.", True)
gen.add("shutdown_costmaps", bool_t, 0, "Determines whether or not to shutdown the costmaps of the node when move_base is in an inactive state", False)

gen.add("oscillation_timeout", double_t, 0, "How long in seconds to allow for oscillation before executing recovery behaviors.", 0.0, 0, 60)
gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10)

gen.add("make_plan_clear_costmap", bool_t, 0, "Whether or not to clear the global costmap on make_plan service call.", True)
gen.add("make_plan_add_unreachable_goal", bool_t, 0, "Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.", True)

gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)

#sukai pid 2022008-29last,3def,min,mx,eb.yaml
gen.add("pid_kp", double_t, 0, "Restore to the original configuration", 0.5,0.01,1.9)
gen.add("pid_kd", double_t, 0, "Restore to the original configuration", 0.5,0.01,1.9)
gen.add("pid_ki", double_t, 0, "Restore to the original configuration",  0.1,0.01,1.9)
gen.add("pid_time_out", int_t, 0, "Restore to the original configuration", 200,50,500)
gen.add("pid_xy_goal_tolerance", double_t, 0, "Restore to the original configuration", 0.01,0.001,1)
gen.add("pid_yaw_goal_tolerance", double_t, 0, "Restore to the original configuration", 0.01,0.001,6.28)
gen.add("pid_tolerate_xy_goal_tolerance", double_t, 0, "Restore to the original configuration", 0.02,0.001,1)
gen.add("pid_tolerate_yaw_goal_tolerance", double_t, 0, "Restore to the original configuration", 0.02,0.001,6.28)
gen.add("pid_distance_threshold", double_t, 0, "Restore to the original configuration", 0.35,0.05,0.5)
gen.add("pid_isStartPid", bool_t, 0, "Restore to the original configuration", True)
gen.add("ispubobstacle", bool_t, 0, "Restore to the original configuration", True)

'''
#exit(gen.generate(PACKAGE,"node_DynamicParam","DynamicParam")) 中，第二个参数表示动态参数运行的节点名，第三个参数是生成文件所使用的前缀，需要和配置文件DynamicParam.cfg名称相同。
exit(gen.generate(PACKAGE, "ContnavGlobalPlanner_node", "ContnavGlobalPlanner"))

#启动可视化界面：
#rosrun rqt_reconfigure rqt_reconfigure
#利用javascript与html还有roslib开源包写一个类似rqt_reconfigure的动态参数的客户端